from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetRemap
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
import os
 
def generate_launch_description():
 
    ## ***** Launch arguments *****
    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')
 
    ## ***** File paths ******
    pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
   
 
    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 'revo_lds.lua'],
        remappings = [
            ('scan', '/scan1')],
        output = 'screen'
        )
 
    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': False},
            {'resolution': 0.05}],
        )
    
    rviz_node = Node(
        package='rviz2',
        namespace='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', '/home/elf/.rviz2/carto.rviz'],
        output='screen')
    return LaunchDescription([
        use_sim_time_arg,
        # Nodes
       # robot_state_publisher_node,
        rviz_node,
        cartographer_node,
        cartographer_occupancy_grid_node,
        # Node(
        #     package='tf2_ros',
        #     executable='static_transform_publisher',
        #     name='laser_to_base_link',
        #     arguments=['0', '0', '0', '0', '0', '0', '/laser', '/base_link']
        # ),
    ])
 